
package JET.physics.bodies;

import JET.geom.Vec2d;
import JET.physics.Border;
import JET.physics.BoundingArea;
import JET.physics.BoundingBox;
import JET.physics.Mass;
import JET.physics.PhysicsHelper;
import java.awt.Color;
import java.util.ArrayList;

/**
 *
 * @author 057Ry
 */
public final class RigidBody extends Body {

    static int rbCount;
    int id;
    private String name;
    
    /**
     * Base shape is made of list of standard points/vectors,
     * specified in theirs local system.
     * This list is "remembers" beggining shape of a rigid body and is used
     * for defining scene active - dynamic rigid body shape, 
     * via translating and rotating base shape vectors.
     */
    public final ArrayList<Vec2d> baseVecList = new ArrayList<Vec2d>();

    /**
     * Dynamic shape is made of masses, so that standard springs can be conected
     * to rigid body.
     * Theese masses are not being integrated. They are used only to make rigid body
     * scene-active (to make it possible to exchenge energy with scene objects).
     * 
     * Passing masses forces to rigid body is made by applyMassForces() method.
     * So process of acting on rigid body in ex. with springs looks like:
     * 
     *      1) spring acts on both masses, applying forces to them
     *      2) mass passes it's force to rigid body via applyMassesForces() method.
     *      3) integration of physical equations...
     */
    public final ArrayList<Mass> dynVecList = new ArrayList<Mass>();

    /**
     * Extra masses used for extra forces application like from springs.
     */
    public final ArrayList<Vec2d> extraBaseVecList = new ArrayList<Vec2d>();
    public final ArrayList<Mass> extraDynMassList = new ArrayList<Mass>();

    /**
     * Walls building body border.
     */
    public final Border border = new Border();
    public final BoundingArea boundingArea = new BoundingBox();
    public final Vec2d separationV = new Vec2d();

    public Color color = Color.BLUE;

    // Index of material
    private int material;

    public final Vec2d force = new Vec2d();
    public final Vec2d velocity = new Vec2d();
    public final Vec2d position = new Vec2d();
    public double angle = 0.;
    public double torque = 0.;       // Torque
    public double omega = 0.;
    public double mass = 0.;
    public double mI = 0.;

    public RigidBody() {
        material = 0;

        id = rbCount++;
        name = "rigidBody_"+id;
    }

    public int      getID() { return id; }
    public String   getName() { return name; }
    public void     setName(String n) { name = n; }
    public Border   getBorder() { return border; }

    public ArrayList<Vec2d> getBaseVecList() {
        return baseVecList;
    }

    public ArrayList<Mass> getDynVecList() {
        return dynVecList;
    }

    public int getMaterial() {
        return material;
    }

    public void setMaterial(int material) {
        this.material = material;
    }

    public Mass createExtraMass(double baseX, double baseY) {
        extraBaseVecList.add(new Vec2d(baseX, baseY));
        Mass m = new Mass(baseX, baseY, 1.);
        this.baseToScene(m.getPosition());
        extraDynMassList.add(m);
        return m;
    }

    /**
     * Builds rigid body from given mass list.
     * Calculates momentum of interia, position, mass, 
     * stores base shape information and inits dynamic shape information.
     * 
     * @param mList List of masses definig rigid body shape structure.
     */
    public void buildBaseShape(ArrayList<Mass> mList) {
        mI = PhysicsHelper.getMomentOfInteria(mList);
        position.become(PhysicsHelper.getMassCenterOfMasses(mList));
        for(Mass m : mList) {
            Vec2d v = new Vec2d( m.getPosition().getSubbed(position));
            baseVecList.add(v);
            dynVecList.add(new Mass(m));
            this.mass += m.getMass();
        }
    }

    /**
     * Builds rigid body from given list of points (vectors) and mass.
     * Calculates momentum of interia and position.
     * Stores base shape information and inits dynamic shape information.
     *
     * @param vList List of points (vectors) definig rigid body shape structure.
     * @param mass Mass of a rigid body.
     */
    public void buildBaseShape(ArrayList<Vec2d> vList, double mass) {
        mI = PhysicsHelper.getMomentOfInteria(vList, mass);
        mass /= vList.size();
        position.become(PhysicsHelper.getGeomCenterOfVecs(vList));
        for(Vec2d v : vList) {
            Vec2d r = new Vec2d( v.getSubbed(position));
            baseVecList.add(r);
            dynVecList.add(new Mass(v.getX(), v.getY(), mass));
            this.mass += mass;
        }
    }

    /**
     * Applies force to the rigid body in specified point.
     * Note that this method doesn't check wheather the point of application
     * is contained withing rigid body area.
     * 
     * @param f Vector of applied force.
     * @param pos Vector of force application position (in scene system).
     */
    public void applyForce(Vec2d f, Vec2d pos) {
        Vec2d r = pos.getSubbed( PhysicsHelper.getMassCenterOfMasses(dynVecList));
        double dist = r.getMagnitude();

        if (dist >= 0.001) {
            Vec2d norm = r.getNormalized();
            Vec2d perp = norm.getPerp();
            double rotF = perp.dot(f);
            torque += dist*rotF;
        }
        
        force.add(f);
    }

    /**
     * Resets all rigid body forces information to zero.
     * Both local dynamic system (rigid body torque) and
     * scene dynamic system (list of masses).
     */
    @Override
    public void zeroForces() {
        force.zero();
        torque=0.;
        for(Mass m : dynVecList)        m.getForce().zero();
        for(Mass m : extraDynMassList)  m.getForce().zero();
    }

    public void zeroVelocities() {
        for(Mass m : dynVecList)
            m.getVelocity().zero();
    }

    public void damp(double value) {
        velocity.scale(value);
        omega *= value;
    }

    @Override
    public void accumulateExternalForces(Vec2d v) {}

    @Override
    public void accumulateInternatForces() {
        for(Mass m : dynVecList)        applyForce(m.getForce(), m.getPosition());
        for(Mass m : extraDynMassList)  applyForce(m.getForce(), m.getPosition());
    }

    /**
     * Integrates rigid body movement equations.
     *
     * @param dt Time step for integration.
     */
    public void simulate(double dt) {

        //Linear
        Vec2d a = force.getScaled(1./mass);
        Vec2d dv = a.getScaled(dt);
        velocity.add(dv);
        Vec2d dr = velocity.getScaled(dt);
        position.add(dr);
        position.add(separationV);
        
        //Rotary
        double e = torque/mI;
        double dw = e*dt;
        omega += dw;
        double dang = omega*dt;
        angle += dang;
        
        updateDynamicShape(dt);
    }

    /**
     * Updates dynamic shape structure.
     * Sets masses new positions according to integrated rigid body position and 
     * derived angle.
     * @param dt
     */
    protected void updateDynamicShape(double dt) {
        for(int i=0; i<baseVecList.size(); i++) {
            Vec2d v = baseVecList.get(i).getRotated(angle).getAdded(position);
            dynVecList.get(i).getLastPosition().become(dynVecList.get(i).getPosition());
            dynVecList.get(i).getPosition().become(v);

            // Set mass velocity from last and actual mass position and time step
            Vec2d derivedMassVel = new Vec2d(v);
            derivedMassVel.sub(dynVecList.get(i).getLastPosition());
            derivedMassVel.scale(1./dt);
            dynVecList.get(i).getVelocity().become(derivedMassVel);
        }

        for(int i=0; i<extraBaseVecList.size(); i++) {
            Vec2d v = extraBaseVecList.get(i);
            Mass m = extraDynMassList.get(i);
            m.getPosition().become(v);
            this.updateMassData(m);
            this.baseToScene(m.getPosition());
        }
    }

    /**
     * Sets rigid body dynamic shape according to given position and angle.
     *
     * @param p Position to which translate rigid body.
     * @param a Angle to which rotate rigid body.
     */
    public void setPositionAndAngle(Vec2d p, double a) {
        for(int i=0; i<baseVecList.size(); i++) {
            Vec2d v = baseVecList.get(i).getRotated(a).getAdded(p);
            dynVecList.get(i).getPosition().become(v);
        }
    }

    /**
     * Transforms vector from scene system to local (base) rigid body system.
     * Passed argument is changed, no new instance of Vec2d is created.
     * @param v
     */
    public Vec2d sceneToBase(Vec2d v) {
        v.sub(position);
        v.rotate(-angle);
        return v;
    }

    /**
     * Transforms vector from local (base) rigid body system to scene system.
     * Passed argument is changed, no new instance of Vec2d is created.
     * @param v
     */
    public Vec2d baseToScene(Vec2d v) {
        v.rotate(angle);
        v.add(position);
        return v;
    }

    /**
     * Updates mass properties as if it was part of this rigid body.
     * Propetries that are updated are: velocity.
     * It is used for ex. when spring needs to calculate its kd factor...
     * @param m Mass to be updated. It must be in rogid body local system.
     */
    public void updateMassData(Mass m) {
        //m.getVelocity().become(Vec2d.zero);
        Vec2d v = m.getVelocity();
        v.become(m.getPosition());
        v.makePerp();
        v.scale(omega);
        v.add(velocity);
    }

    @Override
    public BoundingArea getBoundingArea() {
        return boundingArea;
    }

    @Override
    public void updateBoundingArea() {
        boundingArea.reset();
        for (Mass m : dynVecList)
            boundingArea.extendToContain(m.getPosition());
    }
}
